using dtPolyRef = System.UInt32;

namespace Recast
{
	public unsafe partial class RecastGlobal
	{
		public unsafe partial class dtFindNearestPolyQuery : dtPolyQuery
		{
			public dtNavMeshQuery m_query;
			public float* m_center;
			public float m_nearestDistanceSqr;
			public dtPolyRef m_nearestRef;
			public float* m_nearestPoint;// [3];
			public bool m_overPoly;
			public dtFindNearestPolyQuery(dtNavMeshQuery query, float* center)/*: m_query(query), m_center(center), m_nearestDistanceSqr(FLT_MAX), m_nearestRef(0), m_nearestPoint(), m_overPoly(false)*/
			{
				m_query = query;
				m_center = center;
				m_nearestDistanceSqr = FLT_MAX;
				m_nearestPoint = rcNewArray<float>(3, rcAllocHint.RC_ALLOC_PERM);
			}
			~dtFindNearestPolyQuery()
			{
				// Defined out of line to fix the weak v-tables warning
				rcFree(m_nearestPoint);
			}
			public dtPolyRef nearestRef()
			{
				return m_nearestRef;
			}
			public float* nearestPoint()
			{
				return m_nearestPoint;
			}
			public bool isOverPoly()
			{
				return m_overPoly;
			}
			public override void process(dtMeshTile* tile, dtPoly* polys, dtPolyRef* Refs, int count)
			{
				dtIgnoreUnused(polys);
				float* closestPtPoly = stackalloc float[3];
				float* diff = stackalloc float[3];
				for (int i = 0; i < count; ++i)
				{
					dtPolyRef Ref = Refs[i];
					//float* closestPtPoly = stackalloc float[3];
					//float* diff = stackalloc float[3];
					memset(closestPtPoly, 0, sizeof(float) * 3);
					memset(diff, 0, sizeof(float) * 3);
					bool posOverPoly = false;
					float d;
					m_query.closestPointOnPoly(Ref, m_center, closestPtPoly, &posOverPoly);
					dtVsub(diff, m_center, closestPtPoly);
					if (posOverPoly)
					{
						d = dtAbs(diff[1]) - tile->header->walkableClimb;
						d = d > 0 ? d * d : 0;
					}
					else
					{
						d = dtVlenSqr(diff);
					}
					if (d < m_nearestDistanceSqr)
					{
						dtVcopy(m_nearestPoint, closestPtPoly);
						m_nearestDistanceSqr = d;
						m_nearestRef = Ref;
						m_overPoly = posOverPoly;
					}
				}
			}
		}
	}
}
